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ABSTRACT 

This paper presents a robust and efficient approach for relative navigation and attitude estimation of spacecraft flying 
in formation. This approach uses measurements from a new optical sensor that provides a line of sight vector from the 
master spacecraft to the secondary satellite. The overall system provides a novel, reliable, and autonomous relative 
navigation and attitude determination system, employing relatively simple electronic circuits with modest digital 
signal processing requirements and is fully independent of any external systems. Experimental calibration results 
are presented, which are used to achieve accurate line of sight measurements. State estimation for formation flying 
is achieved through an optimal observer design. Also, because the rotational and translational motions are coupled 
through the observation vectors, three approaches are suggested to separate both signals just for stability analysis. 
Simulation and experimental results indicate that the combined sensor/estimator approach provides accurate relative 
position and attitude estimates. 

INTRODUCTION 

The vision-based navigation (VISNAV) system described in this paper for formation flying applications comprises 
an optical sensor of a new kind combined with specific light sources (beacons) in order to achieve a selective or 
“intelligent” vision. The VISNAV sensor 1 ’ 2 is made up of a Position Sensing Diode (PSD) placed in the focal plane 
of a wide angle lens. When the rectangular silicon area of the PSD is illuminated by energy from a beacon focused by 
the lens, it generates electrical currents in four directions that can be processed with appropriate electronic equipment 
to estimate the energy centroid of the image. While the individual currents depend on the intensity of the light, 
their imbalances are weakly dependent on the intensity and are almost linearly proportional to the location of the 
centroid of the energy incident on the PSD. The idea behind the concept of intelligent vision is that the PSD can be 
used to see only specific light sources, accomplished by frequency domain modulation of the target lights and some 
relatively simple analog signal processing (demodulation). The light is produced by LEDs (beacons) modulated at an 
arbitrary known frequency while the currents generated are driven through an active filter set on the same frequency. 
Calculating the current imbalances then yields two analog signals directly related to the coordinates locating the 
centroid of that beacon’s energy distribution on the PSD, in a quasi-linear fashion, and therefore to the incident 
direction of this light on the wide-angle lens (which gives a line of sight vector). A more detailed description of the 
VISNAV system can be found in Refs. [1-2]. 

Because the beacons are offset from the mass center of the secondary satellite, the observed line of sight couples 
the rotational and the translational motion. The traditional Kalman filter uses this raw information to update the 
attitude and the position equations without any discrimination about the nature of the signal. This approach is 
effective in most of the cases, but it is very difficult for stability analysis because of the complexity of the system 
in hand and of the way that the Kalman gain is calculated. The approach presented in this paper is based on two 
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Figure 1: Focal Plane Measurement from One Light Source 


special characteristics: the observer uses a constant gain for each parameter to be estimated (suboptimal filter) and 
the incoming signal is split according to the translational and the rotational dynamics. The use of constant gains 
avoids dealing with nonlinear time-varying systems, and the signal separation allows two independent plants where 
the stability analysis is feasible on each one using different approaches. 

The presence of four or more beacons in the sensor head field of view (FOV) assures a deterministic solution for 
the navigation and the attitude problem as well. In this case, the sensor redundancy produces a natural division of 
both dynamics, without any additional mathematical procedures. The filtering stage uses this deterministic solution 
as the VISNAV measurements so that the plant models only play the role of increasing the sensor accuracy. In this 
particular case, the stability can be proven by adopting a constant gain in the updating equation suggested in the 
present paper. Three methods for signal separation are given, one in time domain and two in the frequency domain. 
The first method is based on the signal magnitude difference produced by the navigation and the attitude at the 
sensor head location. The other two approaches use the frequency differences of both motions. 

The organization of this paper proceeds as follows. First, the basic equations for the VISNAV system are given. 
Next, the experimental calibration procedure is shown. Then, the relative attitude equations are derived, followed 
by a presentation of the orbital equations of motion. The suggested methods for the signal separation process are 
then presented. Next, the observer design for relative attitude and position estimation is shown. Finally, simulation 
results for formation flying applications are presented. 


BASIC EQUATIONS 


In this section the mathematical models are presented in the context of the particular problem related to relative 
position and attitude estimation from line of sight observations. The notation 3 used in the derivations is briefly 
revisited for the sake of clarification. The angular velocity of the a frame with respect to the 0 frame is represented 
by the physical vector c jp a ( physical denotes that the vector is independent of the frame, whereas mathematical 
denotes the physical vector components expressed in some frame) . The vector is the mathematical vector made 
up of the components of cjp a taken in the 7 frame. The derivative with respect to time is indicated by the operator 
p, where p a R is the rate of change of the vector R relative to the frame a, and pR“ is the time derivative of the 
vector expressed in the a frame. 

Measurement Equation 

Figure 1 shows the focal plane measurement of the VISNAV system for a master and secondary satellite system using 
one light source from a beacon (see Ref. [1] for more details). Three frames are used to describe the orientation and 
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position of the master and secondary satellites. The first one, denoted by (X s , Y s , Z s ), is fixed on the secondary 
satellite, with the LED beacons firmly attached to the body of the satellite, and having known positions in the 
(X s , Y s , Z s ) frame. This frame is also the reference frame for the attitude problem. We assume that this frame is 
centered at the mass center of this spacecraft, and is denoted using the superscript s on the mathematical vectors. 
The second reference system, denoted by (X/, Yf, Zf ), is fixed on the master satellite, where the focal plane of the 
VISNAV system is located. We assume that the Zf axis is along the boresight, passing through the input pin hole 
which is at a distance Zf = +f from the focal plane. The axes Xf and Yf are arbitrary, but fixed in the VISNAV 
sensor. This frame is denoted as the / frame. The third frame, denoted by (X m , Y m , Z m ), is fixed to the mass 
center of the master satellite. The position and orientation of this frame with respect to the focal frame is assumed 
to be known. The vectors for the master frame are identified with the superscript m. 

The point S is the origin of the frame s. The point O is the location of each light beacon in the secondary satellite; 
normally there are several beacons to assure continuous tracking of the satellite and for redundancy. The point I is 
sometimes referred as the image center since it is the intersection of each light beam from the beacon with the focal 
plane, where position of I with respect to the focal reference system is used to form a line of sight observation. The 
point denoted as F in Figure 1 is the pinhole which is coincident with the sensor principal point. Three vectors are 
now defined: SO (the vector from the center S of the s frame to the beacon location O), SI (the vector from the the 
center S of the s frame to the image center I), and 01 (the vector from the beacon location O to the image center 
J, with the constraint equation given by OI = SI — SO. 

The orientation between the secondary and master frames is denoted by the (unknown) rotation matrix C™ which 
transforms a vector expressed in the secondary frame s to the primary frame m. The rotation matrix CJ 1 between 

the focal and the master frames is known by ground calibration. Expressing the vectors SI, OI and SO in frame 
components gives the following relation (colinearity equations) 4 

c s m (s7 -soy = c™v s = v m = (o7) m (1) 

where v s = £ _1/2 [X/ - xo, Yj - y 0 , Z t - z 0 ] T and £ = (X/ - xo ) 2 + (17 - Do) 2 + {Zi ~ zo) 2 ■ The quantity 
(Xo, Yo, Zo) represents the known beacon location, and (X/, Yj, Zj) is the unknown position with respect to the 
secondary satellite. The measurements xj and yj in the focal frame can be expressed in unit vector form by 
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where / is the known focal distance. This unit vector in the master frame is expressed using the fixed rotation 
matrix between the sensor plane frame and the master satellite reference frame, with v m = A bias offset 

in the measurement is also accounted for in the model (denoted by A in Figure 1). The bias vector is a constant 
error vector induced by an unbalance of the horizontal and vertical gains in the focal plane detector relative to the 
particular coordinate system associated with the detector at calibration. Essentially this is the same offset between 
the “electrical center” (zero voltage imbalance) and the geometrical center associated with the optical boresight and 
sensor coordinate system. This vector is denoted by v a and is normally referenced in the focal plane frame: 


= Cfv{ = Cf 
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Finally, the measurement equation for each light source from a beacon, placed on the secondary satellite, is as follows: 


= CT 


'V • + v 
3 ^ a 


for j = 1, ... ,N 


(4) 


where N is the number of LED beacons. 

Small separations between light beams from multiple LEDs reduces the discrimination of each beacon, which 
ultimately produces a dilution of precision in the position and attitude solution. A larger distance between the 
satellites also leads to a dilution of precision since the beacons ultimately approach angular co-location. If the relative 
position between satellites is known then only two non-colinear line of sight vectors are required to determine an 
attitude solution. In a similar fashion for the position navigation only problem, where the satellite is considered to be 
a “mass point” (in other words without attitude), two line of sight vectors are only required. A covariance analysis 
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shows that when the relative position and attitude both are unknown then two line of sight vectors provide only one 
axis of attitude and one axis of position information. 5 Furthermore, an observability analysis using two line of sight 
observations indicates that the beacon that is closest to the target provides the most attitude information but has 
the least position information, and the beacon that is farthest to the target provides the most position information 
but has the least attitude information. In order to find a deterministic solution for the position and velocity at least 
four vector observations are required. 


VISNAV SENSOR CALIBRATION 


In order to use the sensor effectively, an accurate mapping between the normalized voltages ( V x ,V y ) returned by the 
sensor and the location of the light centroid on the image plane ( x , y) must be known. The projection equations 
in Eq. (4) represent the ideal case for a pin-hole camera model. However, in practice the lens and PSD detector 
nonlinearities cause any camera to depart from this ideal model. Therefore a correction of all non-ideal effects must 
be applied into a calibration process that is implicitly constrained to be consistent with the colinearity equations. 
The PSD sensor experiment configuration for the calibration process places the camera at many known positions 
and attitudes ( X C j , Y C j , Z C j , <j>j , 8j , tpj) relative to an array of targets located at (X* , Yj , Zi) . Unfortunately, we must 
also consider the realistic uncertainty in the camera position, orientation and target location since only certain 
levels of precision in the laboratory setting can be achieved. The current experimental configurations are as follows: 
the focal length of PSD sensor / = 0.01m; beacon position = [1.5716m, 0, 0], sensor position = [0, 0, 0]; roll = 
[—50° : 1° : 50°], pitch = [—50° : 1° : 50°], yaw = 0°; total test points = 10201, total test points in the FOV = 
6635. Figure 2(i) represents the ideal location of the light centroid corresponding to each test point configuration. 
The acquisition of the calibration data is computer-automated: a precision two axis air bearing permits the sweeping 
of the sensor over known angles and image-fixed targets. The actual experimental results in Figure 2(ii) show the 
presence of high systematic distortions associated both with a wide angle lens and electronic nonlinearities, which 
must be calibrated. 

One method to accomplish the calibration involves a direct interpolation using the experimental data set. An- 
other way involves determining the global calibration function mapping of the normalized voltages (V x , V y ) into the 
corresponding known ideal locations of the light centroid on the image plane (x,y) for the whole FOV data. Yet 
other way is to determine the local calibration functions available for the corresponding local part of the FOV region. 
The local calibration functions can capture the fine structure of the distortion. In order to compare the optimality of 
the proposed methods, it is necessary to compare the calibration accuracy, memory requirement and computational 
load. Here, we discuss how to determine the global calibration functions. The problem involves determining the 
optimal coefficient sets dij , bij for the bivariate calibration functions 


= fx(V X ,Vy) W EE a ij4 > ijiYx)Vy) — 

2=0 j = 0 2=0 j= 0 


y — fy(Vx,Vy) « EE bij <f>ij (V X > Vy ) EE bijTi-jiV^TjiVy) 
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(5a) 

(5b) 


where n is the order of the univariate element polynomials, <f>ij(V x , V y ) is a bivariate polynomial basis, and T»( V x ), 
Ti(V y ) are the univariate first type Chebyshev polynomials. Let the vector form of the complete set of the basis 
function be given by 


<P(V X ,Vy) = [4> 0 o{V X ,Vy), ct> 10 (V x ,Vy), fa (V X ,Vy), <ho{V X ,Vy) <Pnn(V X , Vy)} 1 


( 6 ) 


Then, our goal is to determine the two coefficient sets 

(7a) 
(7b) 


to minimize the weighted least squares magnitude of the residual vectors R^, and R y : 

R-x = (gx - Ha) 

R y = (g y - Hb) 


a — [floo, Quo, a 11, <220: • • • > Onn] T 
b = [&oo: bio, bn, &20 
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(8a) 

(8b) 



where H = [<p(V x i,V y i)\(j)(V X 2 ,Vy 2 )' ••• : (j>(V x ,V y )] T and g x , g y are the vector forms of the ideal location of the 
light centroid calculated by the colinearity equations. Consequently, the best coefficient sets are determined by 

a = ( H T WH)~ 1 H T Wg x 
b = (. H T WH)~ l H T Wg y 

where W is typically the inverse of the measurement covariance matrix. 

Thereafter, only the coefficient sets are needed to evaluate (a :,y) from the PSD sensor output (V x , V y ). Figure 3 
shows that the results of the 35 th -order calibration functions. The standard deviation of the x errors is 6.77 x 10~ 6 m 
and the standard deviation of the y errors is 5.95 x 10“ 6 m, which indicates an accurate calibration. Figure 4 shows 
that the calibration error decreases as the order of polynomial functions for the calibration function increases. These 
experimental results indicate that the sensor can be accurately calibrated using polynomial functions. 
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Figure 2: Ideal Location and Actual PSD Sensor Output 


Relative Attitude Equations 

In this section the governing equations for the relative attitude kinematics between two bodies are reviewed. The 
kinematic equations presented here are derived using non-inertial reference frames, however only minor changes are 
required from the standard formulation. 3 Starting from Eq. (1) and taking derivative of each vector with respect to 
the same frame in which they are expressed gives the following expressions 

p W m = C rn pw s + = (jp + C s mP Cfw s ) ( 10 ) 

The bias in Eq. (1) is considered to be a constant, so it’s derivative is zero. The same expression in Eq. (10) can be 
derived by the application of the transport theorem, which yields the following expressions 

p m v = p s v + w m5 x v (11a) 

pv m = C™(pv s + [ < s x]v s ) (lib) 

where the matrix [- x] denotes the cross product matrix. 6 
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(i) Calibrated ( x,y ) 


(ii) Approximated Absolute Calibration Error 


Figure 3: Experimental Calibration Results 



Figure 4: Order of Polynomials Versus Calibration Accuracy 


Both expressions, (10) and (11), must be equivalent. Setting these equations equal to each other yields the time 
rate of change of the attitude matrix, given by 

C s mP C? = [< s x] (12a) 

PC? = C7K*. x] = -K,x]Cr (12b) 

The relative attitude kinematics are described by the expression in Eq. (12) in terms of attitude matrix and the 
angular velocity between both frames. 
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Figure 5: Relative Navigation: Master and Secondary Satellite Orbits 


We now write the expression in Eq. (12) in terms of the corresponding quaternions. 7 Toward this end, the 
quaternion is expressed as, q) n = [e 7 sin ~ cos f ] T ; where e is the eigenaxis between both frames and a is the 
rotation angle measured from frame m to frame s. The quaternion is a vector with the same components in both 
the m and s frames, and can be expressed in any external frame as an arbitrary (i.e. general) vector. This has an 
advantage over the rotation matrix formulation, which is fixed to the reference system s and m in this case. 

An infinitesimal rotation is expressed in terms of the quaternion as = 1 + \u3 sm dt, where dt is the time 
differential. Multiplying by the quaternion q) r( and taking the first-order infinitesimal part, the following differential 
equation is given 
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PQm 
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T 

where the quaternion q^ is decomposed into a scalar and a vector part as q^ = \{Q s m ) T <?o] , and [ex] is the skew 
symmetric cross product matrix. Both the attitude matrix and quaternion formulations will be used in the definition 
of the observer feedback error, but the quaternion formulation is used in the actual implementation of the observer. 


Relative Navigation and Dynamics Equations 

From basic orbit theory, 8 the equations of motion are written assuming that each satellite is referenced with respect 
to the same inertial frame. The vectors are described in the Figure 5. The relative orbit is described by the difference 
between both vectors, r = R m — R s . If the master satellite position vector is written as R m = R m [1,0,0] , the 
expression can be simplified. The frame with this property is the Local Vertical Local Horizontal (LVLH) reference 
frame, 9 which is widely used to reference Earth Pointing satellites. The vector r is decomposed in m frame components 
and takes the final expression given by 
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(14) 


where Aa is the relative acceleration. The forcing part along the X axis component has the following structure: 
[/ (R m + ri 2 ) — / (R m )] m which is not robust from a numerical point of view. This expression is maintained for 
compactness and will be used in the observer analysis, but for practical implementations it is convenient to re-write it 
avoiding the subtraction of two large numbers. 8 Equation (14) expresses the dynamical model for relative navigation 
between the secondary satellite with respect to the master satellite. We note that the number of master satellite orbit 
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parameters computed on the ground and to be used in Eq. (14) is at most 3. For the general case, the magnitude 
R m , the angular velocity lj, and angular acceleration ui are just needed. For the special case involving circular orbits, 
only the position magnitude is necessary. 

In the attitude problem, Euler’s equation or the measured gyro outputs are the starting point for the derivation 
of the rotational dynamics equation to obtain the angular velocity between the inertial frame and a body frame. For 
the relative attitude problem, Euler’s equation must be applied in a differential mode, similar in fashion as the orbit 
case. However, we seek an expression without an additional “third” frame (inertial one included), in addition to the 
to and the s frames, so that the system is independent of the extra reference frame’s choice. In other words, the 
relative navigation and the relative attitude must be a function only of the definition of the master and secondary 
frames and completely independent of the particular choice of the inertial frame or any other frame other than m 
and s. This simple fact is common in control theory, where the error or its derivative is only defined by the current 
and the desired state independent of any other frame choice. 

In the two body problem previously derived, the equation for r is very accurate because it is supported by 
well known models for almost all involved forces in hand, with any remaining small perturbation bounded. In the 
relative attitude dynamics the presence of internal torques, which are normally unmodeled with an unbounded time 
integral, plays an important role in the model equations. We assume that each satellite in the constellation has an 
attitude control subsystem able to maintain the desired satellite orientation inside of some allowable bound. The last 
hypothesis is a qualitative one. We assume that the measurements are available frequently enough to use simpler 
propagation models (to be derived) as a function of the sampling interval. A detailed derivation of the relative 
attitude dynamics equations can be found in Ref. [6]. 

SIGNAL SEPARATION 

Due to the slow moving dynamics in orbital motion, the translational and rotational dynamics are almost independent 
(in fact there is a second order coupling thought the angular velocity which appears in both systems, but for this 
analysis we can assume a negligible coupling between both motions). This section presents three approaches for 
signal separation of the translational and rotational dynamics: 

1. Time Domain Approach : This approach assumes that the contribution of the angular rotation is smaller than 
the translational motion contribution on the sensor output. In other words the distance between the beacon 
location and the secondary satellite mass center is much smaller than the distance between the mass center of 
both satellites. 

2. Spline Wavelets Filter. This approach is a frequency band filter which can run in quasi-real time, and uses 
splines as the basis functions for which the incoming signal is represented. 

3. Standard Band Pass Filter : This is the standard approach from basic signal processing theory and in general 
does not run in real time. It uses the different frequency scales between the rotational and translational motion 
to separate them via one or two band filters. 

Time Domain Approach 

Consider r as the effective position vector between the sensor head and the beacon, which couples the orbit and the 
attitude dynamics: 

r = r° + r a (15) 

where the superscripts (-)° and (•)“ denote orbit and attitude respectively. The first and second derivatives of above 
equation are given by r = r° + r“ and r = r° + r a . For notation simplicity (*) denotes the time derivative relative to 
the frame where the forcing function is expressed. It is assumed that all vectors are expressed in the master satellite 
body frame without loss in generality. The solution of this differential equation is given by 

ptk rtk 

r(t k ) =r(tj) +T(tj)(t k -tj) + F(r°,r°, t) (t k - tj)dt + / P (r a ,r°, t) (t k - t 3 ) dt (16) 

Jtj Jtj 

where F(-°) represents the orbit dynamics and P(-°) denotes the attitude dynamics which is considered as a pertur- 
bation of the main motion. In other words, the contribution of the navigation signal is much important in magnitude 
than the rotational motion, which allows us to split one from the other. 
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The reference problem is computed as 


r r (h) =r r ( tj ) +i r ( tj)(t k 


t ) (tfc - tj) dt 


where the superscript (-) r denotes the reference value. In order to solve the signal separation problem an extra 
(weak) condition is required: the reference and the effective vectors are coincident at each sampling time, i.e., r = r r 
and r = r r . The distance of the * th component of (r — r r ) is expressed as the following integral 


(r - r r )i = [ $ (t) (t k - tj) dt. 

Jt , 


The disturbance function is defined as 

% ( t ) = T (r°, r°, t)-T (r ro , r r °,t)+V (r“, r a , t) = AF tj ^ t (■) + V t (•) - (19) 

where T and V represent the i th component of F and P, respectively. The residual function R tj ^t k is now defined 
as 

& $ t . (t)dt 

( 20 ) 

tk tj 

There are several methods to compute the residual function, but for a small sampling time the overall error is 
almost independent of the approach selected. A quadratic linear function is used because for simplicity, given by 

$ tj (t) = a + b (tj —t) + c (tj - t ) 2 (21) 

where a , b and c are the linearly embedded coefficients to be computed. The polynomial coefficients can be calculated 
using three points, denoted at times t k - 1 , t k and t k +i . The approximate functions are written as 


Rtk -> £fc-i 
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l/8$ 4fc (tk-i) 5/12$ tfc (t k ) -1/24 $ th (tk+i) 
_1_ 7/24 * tk+1 (tk-i) 1/4 Ztu-Ah) -1/24#^ (t k+1 ) 

At* -1/24 * tfc+1 («*_i) 1/4 $ tk+1 (h) 7/24$ tk+1 (t k+ i) 
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We now have 
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where the matrix M. is given by 


A t~ 2 Rt k ^t k ^! - l/8AR tk -+t k - 1 + 1/24A Tt k -yt k+1 
ka _ A t~ 2 R tk _^t k - l/4AJ r tjt _ 1 _^ tit + l/24AR tk _ 1 -y tk+1 

At~ 2 Rt k+1 -n k + l/24AiF tfc+1 _ +tfc l - 1/4A T tk+k -±t k 
_ At~ 2 R tk ^ tk+1 + l/24AR tk ^ tk _ 1 - l/8AR tk -yt k+1 _ 

The minimum norm solution applied to each one of the above vector equations gives the following results 
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It is well known from the interpolation theory that the minimum error can be expected at the middle point, where 
“the mass center of the data” is allocated, therefore, only V (t k ) is calculated from the above expression to reduce 
the integration error. After the calculation of the vector V (t), the signal proportional only to the attitude motion is 
obtained and the angular displacement can be computed. 
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Spline Wavelets Filter 

In this section, we use 5-splines N m to be the scaling functions. Let Ni be the characteristic function, which is zero 
everywhere except in the finite support [0, 1) where it takes unit value. For each positive integer m, the m th -order 
cardinal 5-spline is defined, inductively, by 


N m (t) = (N m _i 



N m -i (t — x) dx 


(26) 


A fast computation of N m (t) can be achieved by using the following formula recursively until we arrive at the first 
order 5-spline A^: 10 

N m ( t ) = ~ r Nryi—i ( t ) + (t - 1) (27) 

m — 1 m — 1 

One of the most important properties of the 5-spline is “total positivity” (see Ref. [11]) by virtue of which the 
function in terms of a 5-spline series follows the shape of the data. For example, if g(t) — ^ • [a 3 N m ( t — j )] , 
then otj > 0 Vj => g(t) > 0, ctj (increasing) => g[t) (increasing), and atj (convex) g(t) (convex). Furthermore, the 
number of sign changes of g[t) does not exceed that of the coefficient sequence { ay } . 

Any function f(t) G L 2 can be mapped into a spline space of order m as 


m => f M (t) = Y2 c?N m (2 M t -k) e V M (28) 

k 

For to = 1, we have an orthonormal basis (see Ref. [10] for a complete theory). The scaling function coefficients 
are computed from the signal samples using 

fOO _ 

cf = 2 M / f M (t) N m (2 M t - k)dt (29) 

J OO 

where N m (t) is the dual of N m (t) and (•) is the complex conjugate. In practice the signal f(t) is known at some 
discrete points. The given time step determines the scale M to which the function can be mapped. Because of the 
interpolatory representation f(t) = f\{(t) at the sampling points, and because of the polynomial reproducibility, the 
representation is exact at every point for a polynomial of order m if the basis is N m . In addition, since they are a 
local basis, the representation (28) is also local, which means we only need a few values of the function to obtain the 
coefficients cff for some k. 

For a given 5-spline N m we can construct the corresponding wavelet function ip m with minimal support (for 
more details on wavelet theory see Refs. [11-12]). A function }(t) can be represented in terms of ip as 


/w = E dJ ^( 2 ^- fc ) ( 3 °) 

where 

r OO _ 

d{ = 2 j f(t)p{2H-k)dt (31) 

J OO 

Unlike the total positivity of splines, wavelets have the so-called “complete oscillation” property by virtue of which 
the wavelet coefficients help to detect any change in the function behavior. The algorithm based on a multiresolution 
analysis (MRA) has the following relations 


XM+l(t) € Am+1 , 

=> 

XM+ 1 = Yl c ^ +lNm (2 M+1 
k 

XM(t) e Am , 


X M = E C k 1 ~ k ) 

k 

I/mW € Wm , 


Vm = E (2 Mt - k ) 


k 


Since the MRA requires 11 


Am+i — Am + Wm 


(32) 
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we have 


xm + 1 (t) = x M + l im (33a) 

Ck +lN ™ (2 M+1 t - fc) = ^ cf iV m (2 M t - Jfe) + 5] df V (2 M « - fc) (33b) 

fc k k 

Using the decomposition relation 1 2 gives 

N m (2 M+1 t - Z) = 53 { ft o [2A - Z] AT m (2 M f - fc) + hi [2k - l\ip (2 M t - k) } (34) 

k 

Substituting (34) into (33b) gives an equation in the same resolution M. Finally, the coefficients are computed as 

cf = 53h 0 [2jfc-/]c z M+1 (35a) 

i 

df = 53 h-1 [2fc - l ] cf +1 (35b) 

i 


where ho represents a low pass filter and hi a high pass filter. The input signal is projected in two subspaces Am 
and Wm via the filter ho and hi. In our particular case, these subspaces are the navigation plane (low frequency) 
and the attitude plane (high frequency). By repeating this algorithm, a signal decomposition at various frequency 
octaves is obtained, which is the standard MRA decomposition of any arbitrary function. The objective of this 
analysis is obtain two functions which may or may not be related to octave frequencies. To deal with this problem, 
as interoctave parameter is defined, given by 12 

2 n 

a n = a n ,N = -T7 N > 0 and n = 1, . . . , 2 N - 1 (36) 

n + 2 JV 

which gives 2^ — 1 additional levels between any two consecutive octave levels. 

The original problem of mapping xm to x n M is similar to the standard MRA decomposition. For the special case 
of a B-spline, with m = 2 we have 

4(f) = 53 c k,n N 2 (2 M a n (t)t - k ) (37) 

k 

with 

C k,n = (38) 

The proper selection of n and N allows the separation of any frequency band that is desired. The important fact 
of this interoctave algorithm is the original signal can be projected on each subspace without intersection (i.e., the 
attitude subspace does not receive any navigation signal). 

Standard Band Pass Filter 

In general the navigation motion is dominated by the orbital period of the master satellite which is around 6000 
seconds for Low Earth Orbit (LEO). The attitude motion is related to the internal closed-loop design which is in 
general around a couple of hundred seconds. This frequency distance can be used to filter one motion with respect 
to the other a using standard band filter design. This approach saves previous data to sequentially apply the band 
filters at each time. Therefore, in general, it is more suitable for batch processing than for real time application. 

OBSERVER DESIGN 

In this section an observer is designed to estimate the relative attitude and angular velocity as well as the relative 
position and the linear velocity. In Ref. [5] the information matrix of the attitude and position estimation errors is 
explicitly calculated for two line of sight observations. The information matrix is divided into four partitions, where 
the two main diagonal elements correspond to the attitude and position information matrices that have the identical 
structure if each problem (i.e., attitude or position) was considered independent of each other. The off-diagonal 
partitions couple the attitude and the position errors. A diagonalization (i.e., a decoupling of the attitude and 
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Figure 6: Angular and Position Error Visualization 


position) of the information matrix occurs only in very special cases (the presence of a deterministic solution for 
example). Therefore, the entire problem which includes both attitude and position estimation should be considered 
in the observer design. Toward this end, the signal is separated using the previous methods, and both observers 
(attitude and position) are designed independently. 


Attitude and Navigation Observer 

The observer design treats the attitude portion by representing the residual (measurement minus estimate) error 
through a quaternion vector formulation, and treats the position portion of the residual in a straightforward position 
vector formulation. The angular error between the measured (v m ) and the estimated (v m ) vectors in master frame 
can be “visualized” by a rotation axes normal to plane that contains both vectors. This axis (0J?) can be interpreted 
as the vector part of the quaternion error, and the rotation angle between both vectors is the scalar part of the 
quaternion. The position error (dz) is simple vector difference between the estimated and measured vectors. Figure 
6 shows both approaches. 

Before continuing with this concept, the following matrix relation is first written 


C T = C™C™ = ACC™ 

s m s s 


(39) 


where the estimated vector, matrix or frames are noted with the superscript (•), and CT = AC. The rotation 
error matrix between the estimated and measured quantities can be written in terms of the quaternion as AC = 
I + 2 q 0 g k + 2[gx] 2 . To simplify the notation this matrix is simply defined as AC = (I + [dx])T. Equation (1) can 
now be re-written as 

v m = (/ + [dx])™Cfv s (40) 

where v s is an estimated vector, which depends on only of the angular motion (after signal separation). Equation 

(40) can be re-written in residual form as 

-v™ = [5x]2v™ (41) 

Using the multiplicative property of the cross product matrix the right hand side of Eq. (41) can be expressed in a 
more convenient form as 

v ra -v m = [vxl’ il (Cf (42) 

where the vector r/j™ is expressed as the vector part of a quaternion in any frame. As stated previously this is one 
advantage of using the quaternion parameterization over the rotation matrix in the observer. The left hand side of 
Eq. (42) is denoted by dz = v TO — v m for simplicity. 

The number of measured line of sight vectors is generally greater than one, and the processing of this information 
can be done in the least square sense. Each estimated vector cross product is stacked into a matrix as 


U™ = 


[vix]’ 

[vjvx] 


(43) 


In this case the pseudoinverse is computed using all available information. Therefore, the quaternion error is computed 
by 

U+dz = <j>™ -+[S e ,5q 0 ] (44) 
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Table 1: Orbital Elements of the Master Satellite 


Semimajor axis 
Eccentricity 
Inclination 
Node right ascension 
Argument of perigee 
Mean anomaly 


a = 6, 878 km 
e = 0 
i = 50° 
ft = 5° 
w = 10° 

M = 8° 


where Vf is the pseudoinverse of V ,- n . The computation of the quaternion error is comparable to the algorithm 
presented in Ref. [13], but the scalar part of the quaternion ( Sq a ) is assumed to always be equal to +1. However, the 
scheme presented in this section maintains all four elements of the quaternion error because the sign of the scalar 
part is used in the design of the observer. 

The nonlinear observer presented in Ref. [14] is used for attitude estimation; however, two slight modifications 
are introduced. The first one incorporates an angular velocity model, and the second includes a model of a potential 
bias, represented by b m , in the quaternion differential equation to include any offset of the sensor, which may even 
be the computation of the focal distance. The dynamics of the observer are given by 


b? = l;[qohx3 + [e?x}} 

Z 

r n 

(45) 

x I + b + K vSe sign {6q a ) 

Qo= Resign (Sq 0 ) 

(46) 

£ Cs = “A oC + AT™ + K p Sq sign (Sq 0 ) 

(47) 

X Ttl ^ 

AT = -H AT m + K t 5q sign (6q 0 ) 

(48) 

b = ~M b m + Kbdg sign (Sq 0 ) 

(49) 


where T is the torque difference estimate, is the relative angular velocity, M is a diagonal positive definite 
matrix which represents the time constant of the process, and K V) K p , Kt and Kb are positive definite matrices. 
The sign function ensures that the smallest possible angle is chosen between the two equivalent rotations angles 
described by <f> and 2ir - <f>. The stability proof of this observer can be found in Ref. [6]. 

The observers for the relative position and relative linear velocity are given by 

f = v — K p dz (50) 

v = f (r, v) — K v dz (51) 

where f (•) is the right hand side of Eq. (14), r is the relative position vector, and v is the relative linear velocity 
vector. The minus signs in Eqs. (50) and (51) are due to the definition of dz. The constant gains K p and K v are 
positive definite matrices (usually diagonal). The stability proof of this observer can be found in Ref. [6], 

SIMULATION 

The orbital elements used in the simulation of the master satellite are shown 
perturbation of these elements is used to simulate the motion of the secondary 
both satellites is given by 

I s = I m = diag[100, 120, 130] N-m-s 2 
In the observer the following inertia matrices are used: 

/ s = I m = diag[110, 115, 140] N-m-s 2 (53) 

The true relative initial angular velocity is constant, and given by 

w = [0.065, 0.048, 0.03 ] t deg/sec (54) 


in Table 1. A small initial condition 
satellite. The true inertia matrices of 

(52) 
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(i) Attitude Errors (ii) Angular Velocity Errors 
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e, 

N 



Time (sec) Time (sec) 

(iii) Position Errors (iv) Linear Velocity Errors 


Figure 7: Attitude and Angular Velocity Errors, and Position and Linear Velocity Errors 


The relative angular velocity trajectory is computed by integrating the following equation 

u> = A I~ 1 w (55) 

where I s is the true inertia and A = 0.02. A noise of 0.01/3000 is assumed for each measurement on the focal 
plane. Four beacons are placed on the secondary satellite at a distance of 1 meter from the mass center along each 
coordinate axis. The fourth beacon is placed at [1, 1, 1] T in the secondary frame. 

The observer described in the last section is implemented for state estimation from the line of sight measurements. 
The initial condition angular error is a rotation of about 15° along each of the coordinates axes. The initial angular 
velocity has 50 percent errors from Eq. (54). The initial position condition 10 percent from the true value and the 
initial linear velocity condition is 30 percent from the true value. The sampling rate is 4 Hz. The plots in Figure 
7 show attitude and angular velocity errors, and position and linear velocity errors for the estimator. The relative 
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distance along the X axis is almost three times the distance along the other two axes (around 94 meters against 30 
meters). This difference can be observed in the oscillation of the attitude error in roll, which is intuitively correct. 
The roll angle error is within 0.3 degrees, and the pitch and yaw angles are within 0.05 degrees. The position error 
in all three axis is within 1 cm. Also, the velocities are well estimated using the observer. 

CONCLUSION 

A novel vision-based sensor involving LED beacons and position sensing technology in the focal plane has been 
introduced for formation flying applications. In order to achieve an accurate line of sight measurement from this 
sensor a calibration procedure has been shown. Experimental results indicate that the calibration provides accurate 
results. Also, an observer based system has been presented as an alternative to the extended Kalman filter for 
formation flying navigation of spacecraft. Simulation results have shown that accurate relative attitude and position 
estimation is possible. 
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